using System;

namespace Blimpbots.Control {
    class TwoDimBlimpController {
        private double iTerm;
        private String name;

        private void report(String s) {
            Console.WriteLine("2DC_" + name + ": " + s);
            return;
        }

        public TwoDimBlimpController(String _name) {
            name = _name;
            iTerm = 0;
        }

        public void update(double currentOrientation, double desiredOrientation,
            out double leftMotorSetting, out double rightMotorSetting) {

            double error = normalizeOrientation(desiredOrientation - currentOrientation);

            report("error: " + error);

	        double iFactor = 0;
	        double pFactor = 1; // adjust using signalForMaxTurn instead!

            double signal = error*pFactor + iTerm*iFactor;

            iTerm += error;

            double signalForMaxTurn = Math.PI/2.0;

            double maxMotorSpeed = 4;
            double fullSpeedSpeed = 1;
            
            signal /= signalForMaxTurn;

            double fplusm = maxMotorSpeed + fullSpeedSpeed;
            double mminusf = maxMotorSpeed - fullSpeedSpeed;

            if (signal < -1) {
                report("max right turn");
                leftMotorSetting = maxMotorSpeed;
                rightMotorSetting = -1*maxMotorSpeed;
            }
            else if (signal > 1) {
                report("max left turn");
                leftMotorSetting = -1*maxMotorSpeed;
                rightMotorSetting = maxMotorSpeed;
            }
            else if (signal <= 0) {
                leftMotorSetting = fullSpeedSpeed - mminusf * signal;
                rightMotorSetting = -1 * maxMotorSpeed - fplusm * (-1 - signal);
                report("partial right turn");
            }
            else {
                leftMotorSetting = -1 * maxMotorSpeed + fplusm * (1-signal);
                rightMotorSetting = fullSpeedSpeed + mminusf * signal;
                report("partial left turn");
            }

            report("l: " + leftMotorSetting + " r: " + rightMotorSetting);

            // scheme: go at max speed on both motors if no error
            // otherwise, turn by slowing down the opposing motor
            // for a max turn, motor goes full reverse

            /*report("signal is " + signal);
            if (signal <= deadbandSignal && signal >= -1*deadbandSignal) {
                leftMotorSetting =  fullSpeedSpeed;
                rightMotorSetting =  fullSpeedSpeed;

                report("forward- l: " + leftMotorSetting + " r: " + rightMotorSetting);
                return;
            }
            
            signal *= ( maxMotorSpeed) / signalForMaxTurn;
            if (signal >  maxMotorSpeed)
            {
                signal =  maxMotorSpeed;
            }
            else if (signal < -1*maxMotorSpeed)
            {
                signal =  -1*maxMotorSpeed;
            }*/
            /*
            signal *= (2*maxMotorSpeed) / signalForMaxTurn;
            if (signal > 2*maxMotorSpeed) {
                signal = 2*maxMotorSpeed;
            }
            else if (signal < -2*maxMotorSpeed) {
                signal = -2*maxMotorSpeed;
            }*/

            // we have a range of -max to +max on each motor,
            // for a full range of 4*max
            // (so yeah, its not really a 'max' in that sense)

            // TURN NEEDED:
            // extreme  left turn:  l:-max r:+max  signal: 2*max
            // moderate left turn:  l:0    r:+max  signal: max
            // neutral              l:+max r:+max  signal: 0
            // moderate right turn: l:+max r:0     signal: -max
            // extreme  right turn: l:+max r:-max  signal: -2*max

            // TURN NEEDED:
            // extreme  left turn:  l:-max r:+max  signal: 2*max
            // moderate left turn:  l:0    r:+max  signal: max
            // neutral              l:0    r:0     signal: 0
            // moderate right turn: l:+max r:0     signal: -max
            // extreme  right turn: l:+max r:-max  signal: -2*max

            /*if (signal < 0)
            {
                // turn right
                leftMotorSetting = -.75 * signal;
                rightMotorSetting = 1*signal;
            }
            else
            {
                leftMotorSetting = -1 *signal;
                rightMotorSetting = .75*signal;
            }*/
            /*
            if (signal < 0) {
                // turn right
                leftMotorSetting = maxMotorSpeed;
                signal += maxMotorSpeed; // normalize so we have a range of -max to +max
                rightMotorSetting = signal;
            }
            else {
                rightMotorSetting = maxMotorSpeed;
                signal *= -1; // correct for direction
                signal += maxMotorSpeed;
                leftMotorSetting = signal;
            }*/

            
            

            return;
        }

        public static double normalizeOrientation(double orientation) {
            while (orientation > Math.PI) {
                orientation -= 2*Math.PI;
            }

            while (orientation <= -1*Math.PI) {
                orientation +=  2*Math.PI;
            }

            return orientation;
        }
    }
}